/*
 * @Description: 
 * @Author: Sang Hao
 * @Date: 2021-09-15 17:10:53
 * @LastEditTime: 2021-11-06 20:42:49
 * @LastEditors: Sang Hao
 */

#include "lidar_slam/publisher/cloud_publisher.hpp"
#include "glog/logging.h"

namespace lidar_slam {
// 发布消息
CloudPublisher::CloudPublisher(ros::NodeHandle& nh, std::string topic_name, 
								std::string frame_id, size_t buff_size) 
	: nh_(nh), frame_id_(frame_id) {
	publisher_ = nh.advertise<sensor_msgs::PointCloud2>(topic_name, buff_size);
}

void CloudPublisher::Publish(CloudData::CLOUD_PTR& cloud_ptr_input, double time) {
	ros::Time ros_time((float)time);
	PublishData(cloud_ptr_input, ros_time);
}
void CloudPublisher::Publish(CloudRGBData::CLOUD_PTR& cloud_ptr_input, double time) {
	ros::Time ros_time((float)time);
	PublishData(cloud_ptr_input, ros_time);
}
void CloudPublisher::Publish(CloudData::CLOUD_PTR& cloud_ptr_input) {
	ros::Time time = ros::Time::now();
	PublishData(cloud_ptr_input, time);
}
void CloudPublisher::Publish(CloudRGBData::CLOUD_PTR& cloud_ptr_input) {
	ros::Time time = ros::Time::now();
	PublishData(cloud_ptr_input, time);
}

/**
 * @description: 发布时也要将点云指针CLOUD_PTR包装成sensor_msgs::PointCloud2发出去
 * @param  {*}
 * @return {*}
 * @param {Time} time
 */
void CloudPublisher::PublishData(CloudData::CLOUD_PTR& cloud_ptr_input, ros::Time time) {
	// 新建一个输出的点云指针
	sensor_msgs::PointCloud2Ptr cloud_ptr_output(new sensor_msgs::PointCloud2());
	// toROSMsg将点云转换为ROS消息，即pcl::PointCloud<PointT> -> sensor_msgs::PointCloud2
	pcl::toROSMsg(*cloud_ptr_input, *cloud_ptr_output);
	cloud_ptr_output->header.stamp = time;
	cloud_ptr_output->header.frame_id = frame_id_;
	publisher_.publish(*cloud_ptr_output);
}

void CloudPublisher::PublishData(CloudRGBData::CLOUD_PTR& cloud_ptr_input, ros::Time time) {
	// 新建一个输出的点云指针
	sensor_msgs::PointCloud2Ptr cloud_ptr_output(new sensor_msgs::PointCloud2());
	pcl::toROSMsg(*cloud_ptr_input, *cloud_ptr_output);
	cloud_ptr_output->header.stamp = time;
	cloud_ptr_output->header.frame_id = frame_id_;
	publisher_.publish(*cloud_ptr_output);
}

bool CloudPublisher::HasSubscribers() {
	return publisher_.getNumSubscribers() != 0;
}
} // namespace lidar_slam